
#include "sensor.h"

Sensor::Sensor(void) {}

uint8_t Sensor::getRegValue(uint8_t deviceAddress, uint8_t reg, uint8_t msb, uint8_t lsb)
{
  if ((msb > 7) || (lsb > 7) || (lsb > msb))
  {
    return 0xFF;
  }

  uint8_t rawValue = readRegister(deviceAddress, reg);
  uint8_t maskedValue = rawValue & ((0b11111111 << lsb) | (0b11111111 >> (7 - msb)));

  return (maskedValue);
}

uint8_t Sensor::setRegValue(uint8_t deviceAddress, uint8_t reg, uint8_t value, uint8_t msb, uint8_t lsb)
{
  if ((msb > 7) || (lsb > 7) || (lsb > msb))
  {
    return 0xFF;
  }

  uint8_t currentValue = readRegister(deviceAddress, reg);
  uint8_t newValue = currentValue & ((0b11111111 << (msb + 1)) | (0b11111111 >> (8 - lsb)));

  writeRegister(deviceAddress, reg, newValue | value);
  // 用户添加
  return 0;
}

uint8_t *Sensor::readRegisterBurst(uint8_t deviceAddress, uint8_t reg, uint8_t len)
{
  uint8_t *inBytes = new uint8_t[len];

  Wire.beginTransmission(deviceAddress);
  Wire.write(reg);
  err = Wire.endTransmission(false);

  Wire.requestFrom(deviceAddress, len);

  uint8_t i = 0;
  while (Wire.available())
  {
    inBytes[i++] = Wire.read();
  }

  return inBytes;
}


uint8_t Sensor::readRegister(uint8_t deviceAddress, uint8_t reg)
{
  uint8_t inByte;

  Wire.beginTransmission(deviceAddress);
  Wire.write(reg);
  // Wire.endTransmission(false);
  err = Wire.endTransmission(true);

  size_t size = Wire.requestFrom(deviceAddress, (uint8_t)1);
  if(size != 1)
  {
    err = 0xff;
  }
  inByte = Wire.read();

  return (inByte);
}


void Sensor::writeRegister(uint8_t deviceAddress, uint8_t reg, uint8_t data)
{
  Wire.beginTransmission(deviceAddress);
  Wire.write(reg);
  Wire.beginTransmission(deviceAddress);
  err = Wire.endTransmission(true);
  Wire.write(data);
  err = Wire.endTransmission(true);
}
